﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace FirstPerson
{
    /// <summary>
    /// Implemention of the A star algorithm. how it works
    ///1) Add the starting square (or node) to the open list.
    ///2) Repeat the following:
    /// a) Look for the lowest F cost square on the open list. We refer to this as the current square.
    /// b) Switch it to the closed list.
    /// c) For each of the 8 squares adjacent to this current square …
    ///  If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.           
    ///  If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square. 
    ///  If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.
    /// d) Stop when you:
    ///  Add the target square to the closed list, in which case the path has been found (see note below), or
    ///  Fail to find the target square, and the open list is empty. In this case, there is no path.   
    ///3) Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path. 
    /// </summary>
    class AstarPathfinding
    {
        private List<WayPoint> openList, closedList, nodeList;
        private Stack<WayPoint> path;
        private WayPoint start, goal;
        private const int COST = 10;


        public AstarPathfinding()
        {
            openList = new List<WayPoint>();
            closedList = new List<WayPoint>();
            nodeList = new List<WayPoint>();
            path = new Stack<WayPoint>();

        }

        /// <summary>
        /// Call on this method to get a stack with waypoints that form 
        /// a path from your start to your target waypoint
        /// </summary>
        /// <param name="waypoints"></param>
        /// <param name="startPoint"></param>
        /// <param name="targetPoint"></param>
        /// <returns></returns>
        public Stack<WayPoint> GetPath(WayPoint[] waypoints, WayPoint startPoint, WayPoint targetPoint)
        {

            this.closedList.Clear();
            this.nodeList.AddRange(waypoints);
            this.start = startPoint;
            this.goal = targetPoint;
            this.openList.Add(this.start);

            WayPoint currentNode = null;
            PopulateNeighbors(waypoints);
            float lowf = 10000000;
            do
            {
                lowf = 10000000;
                foreach (WayPoint wp in openList)
                {
                    wp.CalcGFH(this.goal);
                    if (wp.F <= lowf)
                    {
                        lowf = wp.F;
                        currentNode = wp;
                    }
                }
                openList.Remove(currentNode);
                closedList.Add(currentNode);

                foreach (WayPoint wp in currentNode.Neighbors)
                {
                    if (!closedList.Contains(wp))
                    {
                        if (openList.Contains(wp))
                        {

                            if (wp.G > currentNode.G + 10)
                            {
                                wp.ParentPoint = currentNode;
                                wp.CalcGFH(goal);
                            }
                        }
                        else
                        {
                            openList.Add(wp);
                            wp.ParentPoint = currentNode;
                            wp.CalcGFH(goal);
                        }

                    }
                }
            } while (openList.Count != 0 || !closedList.Contains(goal));

            WayPoint wayp = goal;
            while (!path.Contains(start))
            {
                path.Push(wayp);
                wayp = wayp.ParentPoint;

            }


            return path;

            //return new Stack<WayPoint>();
        }

        private void PopulateNeighbors(WayPoint[] waypoints)
        {
            foreach (WayPoint wp in waypoints)
            {
                foreach (int index in wp.AdjacentWaypoints)
                {
                    wp.Neighbors.Add(waypoints[index]);
                }
            }
        }

    }
}
